#ifndef __CONTROLLER_ROS_NODE_H_
#define __CONTROLLER_ROS_NODE_H_

#include <ros/ros.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> 
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>

#include <mc_core/debug/tracked_path.h>
#include <mc_core/robot_model/robot_model_base.h>
#include <motion_controller/controller/motion_controller_define.h>
#include <motion_controller/controller/processor_p2p.h>


using namespace std;

class ControllerRosNode{
    public:
        ControllerRosNode(tf2_ros::Buffer* tf_buffer);
        ~ControllerRosNode();

         // 循环控制
        void Loop();

    private:
        // 地图回调函数
        void MapCallback(const nav_msgs::OccupancyGrid::Ptr& msg);
        // Rviz位姿回调函数
        void RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg);
        // 融合数据回调函数
        void FusionDataCallback(const sensor_msgs::PointCloud2::Ptr& msg);

    private:
        ros::NodeHandle nh_;
        tf2_ros::Buffer* const tfBuffer_;
        ros::Subscriber rvizPoseSub_;
        ros::Subscriber mapSub_;
        ros::Subscriber fusionDataSub_;
        ros::Publisher mapBevPub_;
        ros::Publisher mapDilatePub_;
        ros::Publisher trackedPathPub_;         //
        ros::Publisher robotModelMarkerPub_;    //机器人模型发布者

        OccupancyGridPtr occupancyGrid_;    // 地图数据
        boost::shared_ptr<const geometry_msgs::PoseStamped> goalPosePtr_;   //Rviz位姿
        boost::shared_ptr<sensor_msgs::PointCloud2> fusionDataPtr_;         //传感器融合数据

        boost::shared_ptr<MapBase> mapHandlerPtr_;
        boost::shared_ptr<PlannerBase> globalPathPlannerPtr_;          //路径规划器
        boost::shared_ptr<ControllerBase> localControllerPtr;        //速度规划器
        boost::shared_ptr<StateMachineBase> managerPtr_;          //核心控制器
        boost::shared_ptr<ModelBase> robotModelPtr_;        //机器人模型

        BevMap bevMap_;     //BEV 鸟瞰图
        TrackedPath trackedPath_;       //路径跟踪
        string robotModelConfigFile_;   //机器人模型配置文件

};

#endif